/*******************************************************************************
 * File Name: FlightController_Failsafe.cpp
 *
 * Version: 1.0
 *
 * Description:
 * 	
 * Author: ppks
 *
 * Notes:
 * 	
 *
 ********************************************************************************
 * Copyright (<year>)
 ********************************************************************************
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 ********************************************************************************/

#include "common.h"
#include "FlightController_Failsafe.h"
#include "FlightController_Configuration.h"

/********************************************************************************/
/* Global Variables Declaration                                                 */
/********************************************************************************/
uint16_t FlightController_RCFailsafeCounter = 0;
uint8_t  FlightController_RCFailsafeEvents = 0;

/********************************************************************************/
/* Local Variables Declaration                                                  */
/********************************************************************************/

/********************************************************************************/
/* Local Functions Declaration                                                  */
/********************************************************************************/


/********************************************************************************/
/* Function Code                                                                */
/********************************************************************************/
/********************************************************************************
* Function Name: Failsafe_EnterRCFailsafeState
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
void Failsafe_EnterRCFailsafeState(void)
{
    uint8_t i;

    if (globalFlags.ARMED)
    {
        /* If the system is armed - apply landing RC configuration */
        for (i = 0; i < 3; i++)
        {
            rcData[i] = RC_MIDVALUE;          /* after specified guard time after RC signal is lost (in 0.1sec) */
        }

        rcData[THROTTLE] = FC_Config.MISC.failsafe_throttle;

        if (Failsafe_IsTimeToStopMotors())
        {
            /* Turn OFF motors after specified Time (in 0.1sec) */
            FlightController_SystemDisarm();

            /* This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
             * to restart accidentally by just reconnect to the tx - you will have to switch off first to rearm
             */
            globalFlags.OK_TO_ARM = 0;
        }

        FlightController_RCFailsafeEvents++;
    }
    else
    {
        /* Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
         * This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
         * to restart accidentally by just reconnect to the tx - you will have to switch off first to rearm */
        globalFlags.OK_TO_ARM = 0;
    }
}



